/*******************************************************************************
*   Copyright (C) Harbin Institute of Technology, Shenzhen.
*				  All Rights Reserved.
*   Department: Decoration
********************************************************************************
* File Name   : algorithm.c
* Author      : Dayuan
* Version     : v0.01
* Date        : 2019/9/6
* Description : Decoration Rototics Manipulator task.
*******************************************************************************/
/*-History----------------------------------------------------------------------
* Version   Date      Name        Changes and comments
* v0.01     2019/9/6  Dayuan    initial version
*=============================================================================*/
/* System Includes -----------------------------------------------------------*/
#include <math.h>
/* Public Includes -----------------------------------------------------------*/
#include "main.h"
/* Private Includes ----------------------------------------------------------*/
#include "algorithm.h"
#include "algorithm_if.h"
/* Global variables ----------------------------------------------------------*/
JOINT_INFO joint_info = {2, 0.01, 0.05, 2, 0.1};
//JOINT_INFO joint_info = {1, 0.01, 0.005, 2, 0.2}; //relative stable but not trace well
static PID_EFF pid_eff[2]={{0.02,0.001,0.01},{20,1,0}};
/* Private variables ---------------------------------------------------------*/

/* Global functions ----------------------------------------------------------*/
/**
* @brief  pid control algorithm
* @param  joint motor infomation
* @retval none
*/
void reset_pid_error(JOINT_INFO *joint)
{
	joint->pid_out[CURRENT] = 0;
	joint->pid_out[LAST] = 0;
	joint->angle_err_int = 0;
	joint->f_err[LAST] = 0;
	joint->f_err[CURRENT] = 0;
	joint->f_err_x2[LAST] = 0;
	joint->f_err_x2[CURRENT] = 0;
	joint->inc[LAST] = 0;
}

/**
* @brief  pid control algorithm
* @param  joint motor infomation
* @retval none
*/
void pid_control(JOINT_INFO * joint)
{
	/*
	float error, error_2;
	*/
	static float ki_threshold = 1;
	float ki;
	/*TODO*/
	joint->f_err[CURRENT] = joint->f_des - joint->f_act_switch;
	joint->f_err_x2[CURRENT] = (joint->f_err[CURRENT] - joint->f_err[LAST])*(1/CONTROL_CYCLE);
	
	if(fabs(joint->f_err[CURRENT])>ki_threshold)
		ki = 0;
	else
		ki = joint->ki;
	
	joint->pid_out[CURRENT] = joint->kp * (joint->f_err[CURRENT] - joint->f_err[LAST])
		+ ki *  joint->f_err[CURRENT]															
		+ joint->kd  *(joint->f_err_x2[CURRENT] - joint->f_err_x2[LAST])

		+ joint->pid_out[LAST];
	// constrain limit
//	if(joint->pid_out[CURRENT]>900)
	joint->f_out = joint->pid_out[CURRENT] + joint->f_des;
}

/**
* @brief  pid control algorithm
* @param  joint motor infomation
* @retval none
*/
void pid_position_control(JOINT_INFO * joint)
{
			
	joint->angle_error[CURRENT] = joint->angle_des - joint->inc[CURRENT];
	joint->angle_err_x2[CURRENT] = joint->angle_error[CURRENT] - joint->angle_error[LAST];
	
	joint->angle_err_int = joint->angle_error[LAST];
	
	joint->speed_des[CURRENT] = pid_eff[PID_POSITION].kp * joint->angle_error[CURRENT]
		+ pid_eff[PID_POSITION].ki * joint->angle_err_int
		+ pid_eff[PID_POSITION].kd * joint->angle_err_x2[CURRENT];
/*
	joint->speed_des[CURRENT] = pid_eff[PID_POSITION].kp * joint->angle_err_x2[CURRENT]
		+ pid_eff[PID_POSITION].ki * joint->angle_error[CURRENT]															
		+ pid_eff[PID_POSITION].kd *(joint->angle_err_x2[CURRENT] - joint->angle_err_x2[LAST])
		+ joint->speed_des[LAST];
*/	
	pid_speed_control(joint);
	//joint->speed_des = joint->speed_des[CURRENT] ;//+ joint->f_des;
	
}
/**
* @brief  pid control algorithm
* @param  joint motor infomation
* @retval none
*/
void pid_speed_control(JOINT_INFO * joint)
{
	/*These two condition only occur in speed debug model*/
	if(joint->inc[CURRENT]<-170 && joint->inc[LAST] >170)
		joint->speed_act = (joint->inc[CURRENT]+360 - joint->inc[LAST])*(1/CONTROL_CYCLE);
	else if(joint->inc[CURRENT]>170 && joint->inc[LAST]<-170)
		joint->speed_act = (joint->inc[CURRENT] - 360 - joint->inc[LAST])*(1/CONTROL_CYCLE);
	/**/
	else
		joint->speed_act = (joint->inc[CURRENT] - joint->inc[LAST])*(1/CONTROL_CYCLE);
	update_1th_filter(&joint->speed_fil,joint->speed_act);
	
	joint->f_err[CURRENT] = joint->speed_des[CURRENT] - joint->speed_fil.out;
	joint->f_err_x2[CURRENT] = joint->f_err[CURRENT] - joint->f_err[LAST];

	joint->pid_out[CURRENT] = pid_eff[PID_SPEED].kp * (joint->f_err[CURRENT] - joint->f_err[LAST])
		+ pid_eff[PID_SPEED].ki * joint->f_err[CURRENT]															
		+ pid_eff[PID_SPEED].kd *(joint->f_err_x2[CURRENT] - joint->f_err_x2[LAST])
		+ joint->pid_out[LAST];
	
	joint->f_out = joint->pid_out[CURRENT] + joint->f_des;
}

/**
* @brief  impadence control algorithm
* @param  joint motor infomation
* @retval none
*/
void impadence_control(JOINT_INFO *joint)
{
	/*
	float k = joint_info.k_of_im;
	float d = joint_info.d_of_im;
	im_out = k * theta + d * dtheta
	*/
//	float d_of_im;
	
	joint->angle_error[CURRENT] = joint->angle_des- joint->inc[CURRENT];
	joint->angle_err_x2[CURRENT] = (joint->angle_error[CURRENT]-joint->angle_error[LAST])*(1/CONTROL_CYCLE);
	
//	if(issamesign(joint->angle_error[CURRENT],joint->angle_err_x2[CURRENT])){
//		d_of_im = joint->d_of_im;
//	}
//	else{
//		d_of_im = 0;
//	}
	
	joint->im_fil.in=(joint->k_of_im * joint->angle_error[CURRENT]+joint->d_of_im* joint->angle_err_x2[CURRENT])* TO_RADIUS;
	update_1th_filter(&joint->im_fil,joint->im_fil.in);
	
	/*for offset*/
	
	if(joint->im_fil.out !=0)
	{
		joint->f_des = joint->im_fil.out + joint->gravity.out + joint->im_offset;
	}
	else
	{
		joint->f_des = joint->im_fil.out + joint->gravity.out;
	}
	/*for limit angle*/
	if(joint->f_act > joint->k_of_im*(LIMIT_ANGLE_POS-joint->angle_des)* TO_RADIUS)
		joint->f_act_switch = joint->k_of_im*(LIMIT_ANGLE_POS-joint->angle_des)* TO_RADIUS;
	else if(joint->f_act < joint->k_of_im*(LIMIT_ANGLE_NEG-joint->angle_des)* TO_RADIUS)
		joint->f_act_switch = joint->k_of_im*(LIMIT_ANGLE_NEG-joint->angle_des)* TO_RADIUS;
	else
			joint->f_act_switch = joint->f_act;
	
		//update_1th_filter(&joint->im_fil,im_out);
}
#ifdef PID_DEBUG
/*
* @brief  save current joint info for next cycle
* @param  joint  			joint info of one leg
* @retval none
*/
void desired_generator(JOINT_INFO *joint,uint8_t clear)
{
	static float current_des = 0;
	static float t =0;
	if(clear != 1)
	{		
		current_des = 90*sin((2*PI)/2 *t);
		t+=0.001f;
		joint->angle_des= current_des;
		//joint->f_act = current_des;
	}
	else
		t = 0;
}
#endif
/*
* @brief  save current joint info for next cycle
* @param  joint  			joint info of one leg
* @retval none
*/
void save_joint_info(JOINT_INFO * joint)
{
	joint->pid_out[LAST] = joint->pid_out[CURRENT];
	joint->f_err[LAST] = joint->f_err[CURRENT];
	joint->f_err_x2[LAST] = joint->f_err_x2[CURRENT];
	joint->inc[LAST] = joint->inc[CURRENT];
	joint->angle_error[LAST] = joint->angle_error[CURRENT];
	joint->angle_err_x2[LAST] = joint->angle_err_x2[CURRENT];
	joint->speed_des[LAST] = joint->speed_des[CURRENT];
	}

/**
* @brief  create all filter
* @param  none
* @retval none
*/
void filter_create(JOINT_INFO *joint)
{
	#ifdef FILTER
	init_1th_filter(&joint->angle_fil,0,0);
	init_1th_filter(&joint->f_fil,0,0);
	init_1th_filter(&joint->im_fil,0,0);
	#else
	init_1th_filter(&joint->f_fil,10,joint->f_act);
	init_1th_filter(&joint->im_fil,100,joint->im_fil.in);
	joint->angle_fil.ratio = 1;
	joint->speed_fil.ratio = 0.8;
	#endif
}

/**
* @brief  init filter
* @param  sensor_info         info of sensor
* @param  cutoff              cut off frequency
* @param  value               new value
* @retval filter output
*/
void init_1th_filter(FILTER_INFO *filter, float cut_off, float value)
{
  filter->out = value;
  filter->ratio  = 1.0f/(1.0f/cut_off/2.0f/PI/CONTROL_CYCLE + 1.0f);
}

/**
* @brief  update filter
* @param  sensor_info         info of sensor
* @param  value               new value
* @param	                                      a           
* @param	 CUT_OFF_FREQUENCY = ---------------------------------         
* @param                          (1-a)*2*pi*CONTROL_CONTROL_CYCLE
* @retval filter output
*/
float update_1th_filter(FILTER_INFO *filter, float value)
{
  filter->out = filter->ratio * value + filter->out * (1 - filter->ratio);
  return filter->out;
}

/**
* @brief  smc control algorithm
* @param  joint motor infomation
* @retval none
*/
void smc_controller(JOINT_INFO *joint)
{
	static float e, de, c, k,s,b;
	static FILTER_INFO thetad[2],theta[2];
	static FILTER_INFO dthetad[2]={{0.8,0},{0.8,0}},ddthetad={0.8,0};
	static FILTER_INFO dtheta[2]={{0.8,0},{0.8,0}},ddtheta={0.8,0};
	c = 100;
	k = 5000;
	b = 5000;
	thetad[CURRENT].out = joint->f_des;
	theta[CURRENT].out = joint->f_act;
	update_1th_filter(dthetad,derivate_catch(thetad));
	update_1th_filter(dtheta,derivate_catch(theta));
	update_1th_filter(&ddthetad,derivate_catch(dthetad));
	update_1th_filter(&ddtheta,derivate_catch(theta));
	
	e = thetad[CURRENT].out - theta[CURRENT].out;
	de = dthetad[CURRENT].out - dtheta[CURRENT].out;
	
	s = c*e +de;
	
	joint->f_out = (c*sign(s) + k*s + c*de + ddthetad.out)/b;
	
	thetad[LAST] = thetad[CURRENT];
	theta[LAST] = theta[CURRENT];
	dthetad[LAST] = dthetad[CURRENT];
	dtheta[LAST] = dtheta[CURRENT];
}
/* Private functions ---------------------------------------------------------*/
float derivate_catch(FILTER_INFO* data)
{
	return (data[CURRENT].out - data[LAST].out)*(1/CONTROL_CYCLE);
}

void pid_reset(JOINT_INFO *joint)
{
	joint->pid_out[LAST] = 0;
	joint->pid_out[CURRENT]=0;
	joint->f_out = 0;
	joint->angle_des = 0;
	joint->angle_error[CURRENT] = 0;
	joint->angle_error[LAST] = 0;
	joint->angle_err_x2[CURRENT] = 0;
	joint->angle_err_x2[LAST] = 0;
	joint->f_des = 0;
	joint->f_err[CURRENT] = 0;
	joint->f_err[LAST] = 0;
	joint->f_err_x2[CURRENT] = 0;
	joint->f_err_x2[LAST] = 0;
}

int sign(float data)
{
	if(data>0)
		return 1;
	else if(data<0)
		return -1;
	else
		return 0;
}
/*
int issamesign(float a, float b)
{
	int sign_a, sign_b;
	sign_a = sign(a);
	sign_b = sign(b);
	if((sign_a == sign_b) && (sign_a!=0))
		return 1;
	else
		return 0;
}
*/
/****************************** END OF FILE ***********************************/
